/*!
 * (c) 2006-2008 EPFL, Lausanne, Switzerland
 * Thomas Lochmatter
 */
//#include "includes.h"
//#include "Controller.h"
//#include "khepera3_cpp.h"
//#include "Socket.h"
//#include "ClientSocket.h"
//#include "SocketException.h"

#include <iostream>
#include <string>
#include <cmath>
#include <sstream>
using namespace std;
//
//#include <cstdio>
//#include <cstdlib>
//#include <unistd.h>
//#include <cmath>
//#define PI 3.14159265358979
/*
// Algorithm parameters and results
struct sAlgorithm {
	struct {
		float speed;
		int wall_threshold;
		int turn_count;
		int wait_us;
		int verbosity;
	} configuration;
	struct {
		void (*hook)();
		int turn_counter;
	} state;
};

// Algorithm instance
struct sAlgorithm algorithm;

// Prints the help text.
void help() {
	printf("Template program.\n");
	printf("\n");
	printf("Usage: name_of_your_program [OPTIONS]\n");
	printf("\n");
	printf("Options:\n");
	printf("  -s --speed SPEED      Sets the forward speed (default: 10000)\n");
	printf("  --wall-threshold T    Sets the wall threshold (default: 1000)\n");
	printf("  --turn-count COUNT    Sets the number of steps to turn when hitting an obstacle (default: 50)\n");
	printf("  -w --wait-us US       Sets the waiting time in the perception-to-action loop (default: 50000)\n");
	printf("  -v --verbosity V      Sets the verbosity level (0=quiet, 1=default, 2=verbose, 3=very verbose, ...)\n");
	printf("\n");
}

// State functions
void state_forward();
void state_turn();

// Initializes the algorithm.
void algorithm_init() {
	// Initialize modules
	khepera3_init();

	// Read command line options
	algorithm.configuration.speed = commandline_option_value_float("-s", "--speed", 10000.);
	algorithm.configuration.wall_threshold = commandline_option_value_int("-wt", "--wall-threshold", 1000);
	algorithm.configuration.turn_count = commandline_option_value_int("-tc", "--turn-count", 50);
	algorithm.configuration.wait_us = commandline_option_value_int("-w", "--wait-us", 50 * 1000);
	algorithm.configuration.verbosity = commandline_option_value_int("-v", "--verbosity", 1);

	// Set the initial state
	algorithm.state.hook = &state_forward;
}

// Runs the algorithm by calling the appropriate state function.
void algorithm_run() {
	// Put the wheels in normal (control) mode
	khepera3_drive_start();

	// Execute the current state
	while (1) {
		algorithm.state.hook();
	}
}

void state_forward() {
	// Print the state
	if (algorithm.configuration.verbosity > 0) {
		printf("$STATE,forward\n");
	}

	// Go forward
	khepera3_drive_set_speed_differential(algorithm.configuration.speed, 1., 0.);

	while (1) {
		// Take a measurement with the IR sensors in proximity mode
		// Note that this takes about 3 ms
		khepera3_infrared_proximity();

		// If the front sensors detect an obstacle, switch to turn mode
		if ((khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontSideLeft] > algorithm.configuration.wall_threshold) || (khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontLeft] > algorithm.configuration.wall_threshold) || (khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontRight] > algorithm.configuration.wall_threshold) || (khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontSideRight] > algorithm.configuration.wall_threshold)) {
			algorithm.state.hook = &state_turn;
			break;
		}

		// Print values if necessary
		if (algorithm.configuration.verbosity > 1) {
			printf("$WALL_PROXIMITY");
			printf(",%d", khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontSideLeft]);
			printf(",%d", khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontLeft]);
			printf(",%d", khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontRight]);
			printf(",%d", khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontSideRight]);
			printf("\n");
		}

		// Sleep
		usleep(algorithm.configuration.wait_us);
	}
}

void state_turn() {
	// Print the state
	if (algorithm.configuration.verbosity > 0) {
		printf("$STATE,turn\n");
	}

	// Turn slowly
	khepera3_drive_set_speed_differential(algorithm.configuration.speed, 0., 0.5);

	// Reset the counter
	algorithm.state.turn_counter = 0;

	while (1) {
		// Switch back to forward mode after a certain number of steps
		algorithm.state.turn_counter++;
		if (algorithm.state.turn_counter > algorithm.configuration.turn_count) {
			algorithm.state.hook = &state_forward;
			break;
		}

		// Sleep
		usleep(algorithm.configuration.wait_us);
	}
}


//ODOMETRY

// Instances of the odometry objects
sOdometryTrack ot;
sOdometryGoto og;


void odo_initialize() {
	// Read command line arguments
	//verbosity = commandline_option_value_int("-v", "--verbosity", 1);

	if (algorithm.configuration.verbosity > 0)
			printf("odometry track start");

	// Start a new track
	odometry_track_start(&ot);
	odometry_goto_start(&og, &ot);

	// Put the wheels in normal (control) mode
	khepera3_drive_start();
	if (algorithm.configuration.verbosity > 0)
		printf("odometry init");

}

void odo_goto_position(double& x, double& y)
{
	// Set and announce the new target position
	odometry_goto_set_goal(&og, x, y);
	if (algorithm.configuration.verbosity > 0) {
		printf("$GOTO_POSITION,%f,%f\n", og.state.goal_x, og.state.goal_y);
	}

	// Move until we have reached the target position
	while (og.result.atgoal == 0) {
		// Update position and calculate new speeds
		odometry_track_step(og.track);
		odometry_goto_step(&og);

		// Set speed
		khepera3_drive_set_speed(og.result.speed_left, og.result.speed_right);

		// Print the current position
		if (algorithm.configuration.verbosity > 1) {
			printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
		}

	}

	// Announce when we have reached the target position
	if (algorithm.configuration.verbosity > 0) {
		printf("$POSITION,%f,%f,%f\n", og.track->result.x, og.track->result.y, og.track->result.theta);
		fflush(stdout);
	}
	x = og.track->result.x;
	y = og.track->result.y;
}


*/

#include "ClientSocket.h"
#include "SocketException.h"
	ClientSocket* g_socket=NULL;
bool serverSynchronise()
{
	    try
	    {
	    	cout<<"Trying to create socket!"<<endl;
	      //ClientSocket client_socket( "192.168.0.193", 30000 );
	      g_socket = new ClientSocket( "192.168.0.193", 30000 );
	      cout<<"Socket opened!"<<endl;
	      //g_socket = &client_socket;
	      string reply;

	      try
		{
	    	  //(*g_socket) >> reply;
	    	  //(*g_socket) << "connected";

		}
	      catch ( SocketException& ) {}

	      cout << "We received this response from the server:\n\"" << reply << "\"\n";;

	    }
	  catch ( SocketException& e )
	  {
		  cout << "Exception was caught:" << e.description() << "\n";
		  g_socket = NULL;
		  return false;
	  }
	  return true;
}

#include <vector>
vector<string> tokenize(const string& str)
{
	vector<string> res;
	stringstream ss(str);
	string s;

	while (getline(ss, s, ' '))
	{
	 res.push_back(s);
	}
	return res;
}



#include "Robot.h"
#include "OdometryByModule.h"
// Main program.
int main(int argc, char *argv[]) {
//	// Command line parsing
//	commandline_init();
//	commandline_parse(argc, argv);
//
//	// Help
//	if (commandline_option_provided("-h", "--help")) {
//		help();
//		exit(1);
//	}
//	if(!serverSynchronise())
//	{
//		cout<<"Failed to connect!"<<endl;
//		return -1;
//	}






	if(!serverSynchronise())
			{
				cout<<"Failed to connect!"<<endl;
				return -1;
			}
	string op;
ClientSocket &socket = (*g_socket);

	Robot* robot = Robot::instance();
	OccupancyGrid::instance()->setMetrics(10,10,0.15,0.15);
	AttractivenessMap::instance()->setMetrics(10,10,0.15,0.15);
	Controller* c = Controller::instance();
	IOdometry * odo = new OdometryByModule();
	c->setOdometryModule(odo);
	//AttractivenessMap::instance()->at(start.x,start.y,true);
	//OccupancyGrid::instance()->at(start.x,start.y,false);
	srand(time(0));
	bool origin=true;
	try
	{
		cout<<"start reading instructions!\n";
		do
		{
			cerr<<"waiting\n";

			//getline(socket,op));
			socket>>op;
			//printf("%s",op.c_str());
			cerr<<"command: "<<op<<"\n";

			vector<string> args = tokenize(op);
			cerr<<"args.size "<<args.size()<<':';
			for(int i=0;i<args.size();i++)
				cerr<<'\''<<args[i]<<'\''<<' ';
			cerr<<endl;
			if(args[0]=="go")
			{
				odo->turnTo(M_PI);
				odo->turnTo(0);
				//socket<<"pos\n";
				//socket<<x<<y;
			}
			else if(args[0] == "set_pos")
			{

				int x=atoi(args[1].c_str()),y=atoi(args[2].c_str());
				//socket>>x>>y;
				c->setTile(Tile(x,y));
				if(origin)
				{
					origin = false;
					c->setStart(Tile(x,y));
					AttractivenessMap::instance()->at(x,y,true);
					OccupancyGrid::instance()->at(x,y,false);
				}
				cerr<<"x="<<x<<",y="<<y<<endl;
			}
			else if(args[0]=="set_dir")
			{
				int deg=atoi(args[1].c_str());
				//socket>>deg;
				double angle = deg/360*2*M_PI;
				c->setAngle(angle);

			}
			else if(args[0] == "fight")
			{
				Tile my = c->getTile();
				AttractivenessMap::instance()->at(my.x,my.y,false);

			}
			else if(args[0] == "next_turn")
			{
				cerr<<"preparing turn, calling Tick\n";
				cerr.flush();
				robot->Tick();
				Tile pos = c->getTile();
				stringstream ss;
				ss<<"end_turn "<<pos.x<<' '<<pos.y<<endl;
				socket<<ss.str();
			}
//			else
//
//				socket<<op;
		} while (op!="stop");
	}
	catch ( SocketException& e )
	  {
		  cout << "Exception was caught:" << e.description() << "\n";

	  }



	c->goHome();
	khepera3_drive_set_speed(0, 0);



	return 0;
}
